#include <linux/io.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/proc_fs.h>
#include <linux/seq_file.h>
#include <linux/interrupt.h>
#include <linux/of_address.h>
#include <linux/of_irq.h>
#include <linux/of_platform.h>
#include <linux/of_address.h>
#include <linux/miscdevice.h>
#include <linux/platform_device.h>
#include <linux/module.h>

/*****************************************************************************
 *  Include Section
 *  add all #include here
 *****************************************************************************/
#include   <mach/pmu.h>
#include	<linux/sched.h>
#include <linux/clk.h>

#include "fh_stepmotor.h"


irqreturn_t fh_stepmotor_isr(int irq, void *dev_id)
{
	struct fh_sm_obj_t* obj = (struct fh_sm_obj_t*)dev_id;

	writel(0,obj->regs+MOTOR_INT_EN);
	writel(0,obj->regs+MOTOR_INT_STATUS);
	writel(1,obj->regs+MOTOR_INT_EN);
	
	complete_all(&obj->run_complete);

	return IRQ_HANDLED;
}


int fh_stepmotor_is_busy(struct fh_sm_obj_t* obj)
{
	int busy = readl(obj->regs+MOTOR_STATUS0)&0x01;
	return busy;
}


void fh_stepmotor_set_hw_param(struct fh_sm_obj_t* obj,struct fh_sm_param* param)
{
	unsigned int reg;
	// mode 
	reg = readl(obj->regs+MOTOR_MODE);
	reg &= (~0x3);
	reg |= param->mode&0x3;
	// direction
	if (param->direction)
		reg |= 0x1<<4;
	else
		reg &= (~(0x1<<4));

	// inverse A/B
	if (param->output_invert_A) {
		reg |= 0x1<<5;
	} else {
		reg &= (~(0x1<<5));
	}
	if (param->output_invert_B) {
		reg |= 0x1<<6;
	} else {
		reg &= (~(0x1<<6));
	}
	// microstep
	reg &= (~(0xf<<8));
	reg |= ((param->timingparam.microstep & 0xf)<<8);

#ifdef CONFIG_FH_STEPMOTOR_V1_1
	if (param->keep) {
		reg |= 0x1<<7;
	} else {
		reg &= (~(0x1<<7));
	}
	if (param->initparam.NeedInit) {
		reg |= 0x1<<12;
	} else {
		reg &= (~(0x1<<12));
	}
	reg &= (~(0xff<<16));
	reg |= ((param->initparam.InitRunNum & 0xff)<<16);

	if (param->autostopparam.AutoStopEn) {
		reg |= 0x1<<2;
		if (param->autostopparam.AutoStopSigPol)
			reg |= 0x1<<3; 
		else
			reg &= (~(0x1<<3));

		if (param->autostopparam.AutoStopSigDeb)
#ifdef CONFIG_MC_STEPMOTOR
			writel(1|(1<<1)|(1<<16)|((param->autostopparam.AutoStopSigDebSet&0x3fff)<<2),
				obj->regs+MOTOR_DEBOUNCE);

#else
			writel(1|(1<<16)|((param->autostopparam.AutoStopSigDebSet&0x3fff)<<2),
				obj->regs+MOTOR_DEBOUNCE);
#endif
		else
			writel(0,obj->regs+MOTOR_DEBOUNCE);

	} else
		reg &= (~(0x1<<2));

	fh_pmu_set_stmautostopgpio(obj->id, param->autostopparam.AutoStopChechGPIO);

#endif
#ifdef CONFIG_FH_STEPMOTOR_V1_2
	if (param->dampen) {
		reg |= ((param->dampen & 0x1)<<13);
		reg &= (~(0xf<<24));
		reg |= ((param->damp & 0xf)<<24);
	} else
		reg &= (~(0x1<<13));
#endif
	writel(reg,obj->regs+MOTOR_MODE);
	
	// clk
	reg = readl(obj->regs+MOTOR_TIMING0);
	reg = (param->timingparam.period<<16)|param->timingparam.counter;
	writel(reg,obj->regs+MOTOR_TIMING0); // clk div 10*10

	
	//copy
	reg = readl(obj->regs+MOTOR_TIMING1);
	reg &= (~0xff);
	reg |= (param->timingparam.copy & 0xff);
	writel(reg,obj->regs+MOTOR_TIMING1); // clk div 10*10



	if (fh_sm_manual_4 == param->mode || fh_sm_manual_8 == param->mode) {
		writel(param->manual_pwm_choosenA, obj->regs+MOTOR_MANUAL_CONFIG0);
		writel(param->manual_pwm_choosenB, obj->regs+MOTOR_MANUAL_CONFIG1);
	}

#ifdef CONFIG_FH_STEPMOTOR_V1_1

	if (param->initparam.NeedUpdatePos) {
		reg = readl(obj->regs + MOTOR_INIT);
		reg &= (~0x7);
		reg |= (param->initparam.InitStep)&0x7;
		reg &= (~(0x3f<<8));
		reg |= ((param->initparam.InitMicroStep&0x3f)<<8);
		writel(reg, obj->regs + MOTOR_INIT);
	}

#endif
	//keep param
#ifdef CONFIG_FH_STEPMOTOR_V1_2
	reg = readl(obj->regs+MOTOR_KEEP);
	if (param->keepparam.KeepNumEn) {
		reg |= (param->keepparam.KeepNumEn)&0x1;
		reg &= (~(0xffff<<16));
		reg |= ((param->keepparam.KeepNum&0xffff)<<16);
	} else
		reg &= (~0x1);

	if(param->keepparam.KeepDampEn) {
		reg |= ((param->keepparam.KeepDampEn&0x1)<<1);
		reg &= (~(0xf<<4));
		reg |= ((param->keepparam.KeepDamp&0xf)<<4);
	} else
		reg &= (~(0x1<<1));

	writel(reg, obj->regs + MOTOR_KEEP);
#endif

	
	
	
}

void fh_stepmotor_get_hw_param
	(struct fh_sm_obj_t* obj,struct fh_sm_param* param)
{
	unsigned int reg;
	// mode 
	reg = readl(obj->regs+MOTOR_MODE);
	
	param->mode = reg&0x3;
	param->direction = (reg>>4)&0x1;
	param->output_invert_A = (reg>>5)&0x1;
	param->output_invert_B = (reg>>6)&0x1;
#ifdef CONFIG_FH_STEPMOTOR_V1_2
	param->dampen = (reg>>13)&0x1;
	param->dampen = (reg>>24)&0xf;
#endif


	// microstep
	param->timingparam.microstep = (reg>>8)&0xf;
#ifdef CONFIG_FH_STEPMOTOR_V1_1
	param->keep = (reg>>7)&0x1;
	param->initparam.NeedInit = (reg>>12)&0x1;
	param->initparam.InitRunNum = (reg>>16)&0xff;
	param->autostopparam.AutoStopEn = (reg>>2)&0x1;
	param->autostopparam.AutoStopSigPol = (reg>>3)&0x1;
#endif

	
	// clk
	reg = readl(obj->regs+MOTOR_TIMING0);
	param->timingparam.period = (reg>>16);
	param->timingparam.counter = reg&0xffff;
	
	
	//copy

	reg = readl(obj->regs+MOTOR_TIMING1);
	param->timingparam.copy = reg & 0xff;

	if (fh_sm_manual_4 == param->mode || fh_sm_manual_8 == param->mode) {
		param->manual_pwm_choosenA = readl(obj->regs+MOTOR_MANUAL_CONFIG0);
		param->manual_pwm_choosenB = readl(obj->regs+MOTOR_MANUAL_CONFIG1);
	}

#ifdef CONFIG_FH_STEPMOTOR_V1_1
	reg = readl(obj->regs+MOTOR_INIT);
	param->initparam.InitStep = (reg)&0x7;
	param->initparam.InitMicroStep = (reg>>8)&0x3f;
	reg = readl(obj->regs+MOTOR_DEBOUNCE);
	param->autostopparam.AutoStopSigDeb = (reg)&0x1;
	param->autostopparam.AutoStopSigDebSet = (reg>>2)&0xFFFFFF;
#endif
#ifdef CONFIG_FH_STEPMOTOR_V1_2
	reg = readl(obj->regs+MOTOR_KEEP);
	param->keepparam.KeepNumEn = (reg)&0x1;
	param->keepparam.KeepDampEn = (reg>>1)&0x1;
	param->keepparam.KeepDamp = (reg>>4)&0xf;
	param->keepparam.KeepNum = (reg>>16)&0xffff;
#endif


}



int fh_stepmotor_start_sync(struct fh_sm_obj_t* obj,int cycles)
{
	int fin = 0;
	unsigned int regcycle = 0;
	//cycle
	if (cycles <= 0) return 0;

	reinit_completion(&obj->run_complete);

	cycles -=1;
	regcycle = readl(obj->regs+MOTOR_TIMING1);
#ifdef CONFIG_FH_STEPMOTOR_V1_1
	regcycle = regcycle & (0x000000ff);
	regcycle = regcycle|((cycles<<8)&0xffffff00);
#else
	regcycle = regcycle & (0x0000ffff);
	regcycle = regcycle|((cycles<<16)&0xffff0000);	
#endif

	writel(regcycle,obj->regs+MOTOR_TIMING1);

	writel(1,obj->regs+MOTOR_CTRL);
	wait_for_completion_interruptible(&obj->run_complete);
#ifdef CONFIG_FH_STEPMOTOR_V1_1
	fin = (readl(obj->regs+MOTOR_STATUS0)>>4)&0x1ffffff;
#else
	fin = (readl(obj->regs+MOTOR_STATUS0)>>8)&0x1ffff;
#endif
	
	return fin;

}

void fh_stepmotor_start_async(struct fh_sm_obj_t* obj,int cycles)
{
	unsigned int regcycle = 0;
	
	if (cycles <= 0) return;
	cycles -= 1;
	regcycle = readl(obj->regs+MOTOR_TIMING1);
#ifdef CONFIG_FH_STEPMOTOR_V1_1
	regcycle = regcycle & (0x000000ff);
	regcycle = regcycle|((cycles<<8)&0xffffff00);
#else
	regcycle = regcycle & (0x0000ffff);
	regcycle = regcycle|((cycles<<16)&0xffff0000);	
#endif
	writel(regcycle,obj->regs+MOTOR_TIMING1);
	reinit_completion(&obj->run_complete);
	writel(1,obj->regs+MOTOR_CTRL);

}

#ifdef CONFIG_FH_STEPMOTOR_V1_2
void fh_stepmotor_update_cycle(struct fh_sm_obj_t *obj, int cycles)
{
	unsigned int regcycle = 0;

	if (cycles <= 0)
		return;
	cycles -= 1;
	regcycle = readl(obj->regs+MOTOR_TIMING1);
	regcycle = regcycle & (0x000000ff);
	regcycle = regcycle|((cycles<<8)&0xffffff00);
	writel(regcycle, obj->regs+MOTOR_TIMING1);
}
#endif

int fh_stepmotor_stop(struct fh_sm_obj_t* obj)
{
	int fin = 0;

	if (fh_stepmotor_is_busy(obj)) {
		reinit_completion(&obj->run_complete);
		writel(0,obj->regs+MOTOR_CTRL);
		wait_for_completion_interruptible(&obj->run_complete);
	}

#ifdef CONFIG_FH_STEPMOTOR_V1_1
	fin = (readl(obj->regs+MOTOR_STATUS0)>>4)&0x1ffffff;
#else
	fin = (readl(obj->regs+MOTOR_STATUS0)>>8)&0x1ffff;
#endif
	return fin;
}
int fh_stepmotor_get_current_cycle(struct fh_sm_obj_t* obj)
{
#ifdef CONFIG_FH_STEPMOTOR_V1_1
	int fin = (readl(obj->regs+MOTOR_STATUS0)>>4)&0x1ffffff;
#else
	int fin = (readl(obj->regs+MOTOR_STATUS0)>>8)&0x1ffff;
#endif
	return fin;
}


int fh_stepmotor_set_lut(struct fh_sm_obj_t* obj,struct fh_sm_lut* lut)
{
	
	int i = 0;
	for (i = 0; i< lut->lutsize/sizeof(u32);i++) {
		writel(lut->lut[i],obj->regs+MOTOR_MEM+i*4);
	}
	
	
	
	return 5;
}
int fh_stepmotor_get_lutsize(struct fh_sm_obj_t* obj)
{
	return 256;
}
int fh_stepmotor_get_lut(struct fh_sm_obj_t* obj,struct fh_sm_lut* lut)
{
	int i;
	for (i = 0; i< lut->lutsize/sizeof(u32);i++) {
		lut->lut[i] = readl(obj->regs+MOTOR_MEM+i*4);
	}
	//lut->lut[0] = 0xfedc;
	
	return 5;
}
const u32 fhsm_deflut[] = {
0x00000080,0x0003007f,0x0006007f,0x0009007f,
0x000c007f,0x000f007f,0x0012007e,0x0015007e,
0x0018007d,0x001c007c,0x001f007c,0x0022007b,
0x0025007a,0x00280079,0x002b0078,0x002e0077,
0x00300076,0x00330075,0x00360073,0x00390072,
0x003c0070,0x003f006f,0x0041006d,0x0044006c,
0x0047006a,0x00490068,0x004c0066,0x004e0064,
0x00510062,0x00530060,0x0055005e,0x0058005c,
0x005a005a,0x005c0058,0x005e0055,0x00600053,
0x00620051,0x0064004e,0x0066004c,0x00680049,
0x006a0047,0x006c0044,0x006d0041,0x006f003f,
0x0070003c,0x00720039,0x00730036,0x00750033,
0x00760030,0x0077002e,0x0078002b,0x00790028,
0x007a0025,0x007b0022,0x007c001f,0x007c001c,
0x007d0018,0x007e0015,0x007e0012,0x007f000f,
0x007f000c,0x007f0009,0x007f0006,0x007f0003,
};


void fh_stepmotor_init_hw_param(struct fh_sm_obj_t* obj)
{
	int i = 0;
	// param
	writel(0,obj->regs+MOTOR_MODE);
	writel(0x800000,obj->regs+MOTOR_TIMING0); // clk div 128*1
	writel(0x10010,obj->regs+MOTOR_TIMING1);
	writel(0,obj->regs+MOTOR_MANUAL_CONFIG0);
	writel(0,obj->regs+MOTOR_MANUAL_CONFIG1);
	writel(1,obj->regs+MOTOR_INT_EN);
	// def lut
	for (i = 0; i< 64; i++) {
		writel(fhsm_deflut[i], obj->regs+MOTOR_MEM+i*4);
	}
}


